from launch import LaunchDescription
from launch.substitutions import LaunchConfiguration, Command, PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.parameter_descriptions import ParameterValue
from ament_index_python.packages import get_package_share_directory

def generate_launch_description():
    # 模型文件路径参数
    model = LaunchConfiguration('model')

    # 默认 xacro 路径
    default_model_path = PathJoinSubstitution([
        get_package_share_directory('realsense_gazebo_plugin'),
        'urdf',
        'test.xacro'
    ])

    # 关节状态发布器
    joint_state_publisher = Node(
        package='joint_state_publisher',
        executable='joint_state_publisher',
        name='joint_state_publisher',
        output='screen'
    )

    # 机器人状态发布器
    robot_state_publisher = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        name='robot_state_publisher',
        output='screen',
        parameters=[{
            'robot_description': ParameterValue(
                Command(['xacro ', model]),
                value_type=str
            )
        }]
    )

    return LaunchDescription([
        # 声明模型路径参数
        # 在命令行可以使用: ros2 launch realsense_gazebo_plugin realsense_simulation.launch.py model:=/path/to/file.xacro
        LaunchConfiguration('model', default=default_model_path),
        joint_state_publisher,
        robot_state_publisher
    ])
